#ifndef __FOLLOW_H
#define __FOLLOW_H
#include <iostream>
#include <ros/ros.h>
#include <mutex>
#include <thread>

#include <sensor_msgs/LaserScan.h>
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <geometry_msgs/Point.h>
#include <laser_geometry/laser_geometry.h>
#include <geometry_msgs/Twist.h>

#include <common_msg/TrackedObject.h>
// pcl
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>

#include <costmap.h>
#include <apf.h>
#include <PID_v1.h>

using namespace std;


class Follow : public Costmap, public APFPlanner
{
private:
    void onInit();
    void laserCallback(const sensor_msgs::LaserScanConstPtr& msg);
    void setGoalCallback(const common_msg::TrackedObjectConstPtr& msg);
    void update_map(pcl::PointCloud <pcl::PointXYZ> cloud);
    void get_obstacle(pcl::PointCloud<pcl::PointXYZ> cloud);
    void control_loop();

    /* data */
    ros::NodeHandle nh_;
    ros::Publisher cmd_pub_;
    ros::Subscriber laser_sub_;
    ros::Subscriber goal_sub_;
    tf::TransformListener tf_;
    unsigned char *follow_map_;
    bool missing{true};
    int control_period_ms;
    double angular_gain;
    
public:
    Follow(ros::NodeHandle &nh):nh_(nh){
        onInit();
    };
};
#endif 